#include "status.h"
#include <iostream>

Status::Status(QHostAddress host, quint16 port)
{
  this->host = host;
  this->port = port;
  
  setOrientation(0,0,0);
  
  timeAck = 0;
  connected = false;
  
  ambientPressure = 0;
  depth = 0;
}

int Status::Alive()
{
  timeAck++;
  return timeAck;
}

void Status::update()
{
  timeAck = 0;
}

void Status::getMotor(int index, double* data)
{
  switch(index)
  {
    case 1:
      data[0] = scv1s;//speed
      data[1] = scv1t;//temperature
      data[2] = scv1v;//VIN
      break;
    case 2:
      data[0] = scv2s;//speed
      data[1] = scv2t;//temperature
      data[2] = scv2v;//VIN
      break;      
    case 3:
      data[0] = scv3s;//speed
      data[1] = scv3t;//temperature
      data[2] = scv3v;//VIN
      break;      
    case 4:
      data[0] = scf1s;//speed
      data[1] = scf1t;//temperature
      data[2] = scf1v;//VIN
      break;      
    case 5:
      data[0] = scf2s;//speed
      data[1] = scf2t;//temperature
      data[2] = scf2v;//VIN
      break;      
    case 6:
      data[0] = scls;//speed
      data[1] = sclt;//temperature
      data[2] = sclv;//VIN
      break;      
  }
}

void Status::setMotor(int index, double d1, double d2, double d3)
{
  switch(index)
  {
    case 1:
      scv1s = d1;//speed
      scv1t = d2;//temperature
      scv1v = d3;//VIN
      break;
    case 2:
      scv2s = d1;//speed
      scv2t = d2;//temperature
      scv2v = d3;//VIN
      break;      
    case 3:
      scv3s = d1;//speed
      scv3t = d2;//temperature
      scv3v = d3;//VIN
      break;      
    case 4:
      scf1s = d1;//speed
      scf1t = d2;//temperature
      scf1v = d3;//VIN
      break;      
    case 5:
      scf2s = d1;//speed
      scf2t = d2;//temperature
      scf2v = d3;//VIN
      break;      
    case 6:
      scls = d1;//speed
      sclt = d2;//temperature
      sclv = d3;//VIN
      break;      
  }
}

bool Status::isConnected()
{
  return connected;
}

void Status::setConnected(bool stat)
{
  connected = stat;
}

void Status::setOrientation(double r,double p,double y)
{
  roll = r;
  pitch = p;
  yaw = y;
}

void Status::setTemperature(double temperature)
{
  this->temperature = temperature;
}

double Status::getTemperature()
{
  return temperature;
}

void Status::setPressure(double pressure)
{
  this->pressure = pressure;
}

double Status::getPressure()
{
  return pressure;
}

double Status::getRoll()
{
  return roll;
}

double Status::getPitch()
{
  return pitch;
}

double Status::getYaw()
{
  return yaw;
}

QHostAddress Status::getHost()
{
  return host;
}

quint16 Status::getPort()
{
  return port;
}

void Status::setAmbientPressure()
{
  ambientPressure = pressure;
}

void Status::calcDepth()
{
  depth = (pressure-ambientPressure)/0.014171738; //Divide by psi/cm (density of water at 25 C*gravitational acceleration)
}

double Status::getDepth()
{
  calcDepth();
  
  return depth;
}
